/* 
 * Copyright 2011 kOSMik.
 * This file is part of kOSMik.
 * 
 * kOSMik is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * kOSMik is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with kOSMik.  If not, see <http://www.gnu.org/licenses/>.
 *  
 * kOSMik:
 * Open Street Map - Editor.
 * http://code.google.com/p/swp-dv-ws2010-osm-2/
 */

package de.fuberlin.kosmik.filter;

import static java.lang.Math.pow;
import Jama.Matrix;

/**
 * Thanks to Ahmed Abdelkader for sharing the master of this class at
 * {@link "http://the-lost-beauty.blogspot.com/2009/12/java-implementation-of-kalman-filter.html"}
 * under Creative Commons Attribution 3.0 License.
 * 
 * @author Ahmed Abdelkader, changes by Michael Schmidt
 */
public final class KalmanFilter {

	private Matrix x, x0;
	private Matrix f, b, u, q;
	private Matrix h, r;
	private Matrix p, p0;

	private KalmanFilter() {
		// Hide constructor, use buildKF(...) instead
	}

	/**
	 * Initiate the system matrices (F, B, U, Q, H, R) and initialize the state
	 * and error covariance matrices (X, P).
	 * 
	 * @param x initial value of x
	 * @param y initial value of y
	 * @param dt initial state transition model
	 * @param processNoisePSD process noise
	 * @param measurementNoiseVariance measurement noise
	 * @return initiated kalman filter
	 */
	public static KalmanFilter buildKF(double x, double y, double dt, double processNoisePSD,
			double measurementNoiseVariance) {
		KalmanFilter kf = new KalmanFilter();

		// state vector
		kf.setX(new Matrix(new double[][] { { x, 0, y, 0 } }).transpose());

		// error covariance matrix
		kf.setP(Matrix.identity(4, 4).times(0d));

		// transition matrix
		kf.setF(new Matrix(new double[][] { { 1, 0, dt, 0 }, { 0, 1, 0d, dt }, { 0, 0, 1d, 0 },
				{ 0, 0, 0d, 1d }, }));

		// input gain matrix
		kf.setB(new Matrix(new double[][] { { 0, 0, 0, 0 } }).transpose());

		// input vector
		kf.setU(new Matrix(new double[][] { { 0 } }));

		// process noise covariance matrix
		kf.setQ(new Matrix(new double[][] { { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 1, 0 },
				{ 0, 0, 0, 1 } }).times(pow(processNoisePSD, 2)));

		// measurement matrix
		kf.setH(new Matrix(new double[][] { { 1, 0, 0, 0 }, { 0, 1, 0, 0 } }));

		// measurement noise covariance matrix
		kf.setR(Matrix.identity(2, 2).times(measurementNoiseVariance));

		return kf;
	}

	/**
	 * Estimate next state and covariance.
	 */
	public void predict() {
		x0 = f.times(x).plus(b.times(u));

		p0 = f.times(p).times(f.transpose()).plus(q);
	}

	/**
	 * Updating the prediction by creating a measurement matrix with the
	 * measurements received in Z and passing it to the filter through a correct
	 * call.
	 * 
	 * @param z current measurements
	 */
	public void correct(Matrix z) {
		Matrix s = h.times(p0).times(h.transpose()).plus(r);

		Matrix k = p0.times(h.transpose()).times(s.inverse());

		x = x0.plus(k.times(z.minus(h.times(x0))));

		Matrix i = Matrix.identity(p0.getRowDimension(), p0.getColumnDimension());
		p = (i.minus(k.times(h))).times(p0);
	}

	/**
	 * Returns the vector of the current state.
	 * 
	 * @return state estimate matrix
	 */
	public Matrix getX() {
		return x;
	}

	private void setX(Matrix x) {
		this.x = x;
	}

	private void setB(Matrix b) {
		this.b = b;
	}

	private void setF(Matrix f) {
		this.f = f;
	}

	private void setH(Matrix h) {
		this.h = h;
	}

	private void setP(Matrix p) {
		this.p = p;
	}

	private void setQ(Matrix q) {
		this.q = q;
	}

	private void setR(Matrix r) {
		this.r = r;
	}

	private void setU(Matrix u) {
		this.u = u;
	}
}
